/****************************************************************************
 *
 *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file gps_control.cpp
 * Control functions for ekf GNSS fusion
 */

#include "ekf.h"
#include <mathlib/mathlib.h>

// 控制GNSS关于水平位置、速度、偏航角的融合
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
	if (!_gps_buffer || (_params.gnss_ctrl == 0)) {  // 还有个控制要不要GNSS的全局变量
		stopGpsFusion();
		return;
	}

	if (!gyro_bias_inhibited()) {   // 陀螺仪的三轴零偏都不可用
		_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);  // 设置 EKF-GSF的陀螺仪零偏为状态变量的当前陀螺仪零偏
	}

	// run EKF-GSF yaw estimator once per imu_delayed update
	_yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt,
				imu_delayed.delta_vel, imu_delayed.delta_vel_dt,
				(_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); // EKF-GSF的预测

	_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); // GNSS数据是否存在间隔较长

	// check for arrival of new sensor data at the fusion time horizon
	_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);  // 给定IMU时间戳，从buffer中取出最新的比当前时间戳旧的GNSS数据

	if (_gps_data_ready) {  // 有有效的GNSS数据
		const gnssSample &gnss_sample = _gps_sample_delayed;  // 取出GNSS数据

		if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) {
			if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
				// First time checks are passing, latching.
				_gps_checks_passed = true;  // 时间戳上检查通过以及GNSS数据检查通过
			}

			collect_gps(gnss_sample);  // 初始化NED原点经纬高、位置估计误差，并根据新位置更新磁力计参数和地球自转速率

		} else {
			// Skip this sample
			_gps_data_ready = false;

			if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
				stopGpsFusion(); // 停止融合并重置相关变量
				_warning_events.flags.gps_quality_poor = true;  // 标志位，GPS质量不好
				ECL_WARN("GPS quality poor - stopping use");
			}
		}

		if (_pos_ref.isInitialized()) {  // 如果NED原点已经初始化，则更新GNSS水平位置的辅助变量
			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
		}
		// 速度辅助变量不需要原点已知，直接更新
		updateGnssVel(gnss_sample, _aid_src_gnss_vel);

	} else if (_control_status.flags.gps) {
		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
			stopGpsFusion();  // 虽然有GPS数据，但是数据太久没有更新，可能失效了，停止融合
			_warning_events.flags.gps_data_stopped = true;
			ECL_WARN("GPS data stopped");
		}
	}

	if (_gps_data_ready) {
#if defined(CONFIG_EKF2_GNSS_YAW)  // 如果GNSS有双天线能估计yaw的话这里对其进行融合
		const gnssSample &gnss_sample = _gps_sample_delayed;
		controlGpsYawFusion(gnss_sample);
#endif // CONFIG_EKF2_GNSS_YAW

		controlGnssYawEstimator(_aid_src_gnss_vel);

		const bool gnss_vel_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL));  // 速度辅助变量使能
		const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));  // 水平位置辅助变量使能

		const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
				&& _control_status.flags.tilt_align  // 完成倾斜对准
				&& _control_status.flags.yaw_align  // 完成航向对准
				&& _NED_origin_initialised;        // NED原点已经初始化
		const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;  // GNSS数据检查通过，开始融合条件

		if (_control_status.flags.gps) {
			if (continuing_conditions_passing) {
				if (gnss_vel_enabled) {
					fuseVelocity(_aid_src_gnss_vel);  // 融合速度
				}

				if (gnss_pos_enabled) {
					fuseHorizontalPosition(_aid_src_gnss_pos);   // 融合水平位置
				}

				bool do_vel_pos_reset = shouldResetGpsFusion();  // 返回是否应该重置的标志位

				if (_control_status.flags.in_air
				    && isYawFailure()
				    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
				    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {  // 判别速度是因为使用GNSS估计yaw是通过速度实现的
					do_vel_pos_reset = tryYawEmergencyReset(); // yaw相关判别条件估计失败，则重新估计
				}

				if (do_vel_pos_reset) {
					ECL_WARN("GPS fusion timeout, resetting velocity / position");

					if (gnss_vel_enabled) {
						resetVelocityToGnss(_aid_src_gnss_vel);  // 重置速度和方差至GNSS辅助变量中的保存值
					}

					if (gnss_pos_enabled) {
						resetHorizontalPositionToGnss(_aid_src_gnss_pos); // 重置水平位置和方差至GNSS辅助变量中的保存值
					}
				}

			} else {
				stopGpsFusion(); // 停止融合
			}

		} else {
			if (starting_conditions_passing) { // 第一次，先把相关的重置好
				ECL_INFO("starting GPS fusion");
				_information_events.flags.starting_gps_fusion = true;

				// when already using another velocity source velocity reset is not necessary
				if (!isHorizontalAidingActive()
				    || isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
				    || !_control_status_prev.flags.yaw_align
				   ) {
					// reset velocity
					if (gnss_vel_enabled) {
						resetVelocityToGnss(_aid_src_gnss_vel);
					}
				}

				if (gnss_pos_enabled) {
					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
				}

				_control_status.flags.gps = true; // 重置好后改变标志位供后续融合
			}
		}
	}
}

// 根据GNSS的速度测量更新速度的辅助变量
void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
{
	// correct velocity for offset relative to IMU
	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;

	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;  // 根据机体的角速度和GNSS与IMU之间的位置偏差计算GNSS与IMU之间的速度偏差，用的是
	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;  // 补偿速度偏差

	const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise));   // 速度测量的方差
	const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));              // 三轴速度的方差，垂直方向的标准差放大1.5倍
	updateVelocityAidSrcStatus(gnss_sample.time_us,
				   velocity,                                                   // observation
				   vel_obs_var,                                                // observation variance
				   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
				   aid_src);
}

// 根据GNSS的位置测量更新GNSS水平位置  辅助变量
void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
{
	// correct position and height for offset relative to IMU
	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;  // GNSS位置相对IMU的偏移量（Body），预先标定
	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;  // 偏移量在地球坐标系下的坐标
	const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy(); // GNSS测的的IMU在NED坐标系下的水平位置

	// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
	float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);  // 设置噪声

	if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
		// if we are not using another source of aiding, then we are reliant on the GPS
		// observations to constrain attitude errors and must limit the observation noise value.
		if (pos_noise > _params.pos_noaid_noise) {
			pos_noise = _params.pos_noaid_noise; // 当GPS是唯一的水平辅助源时，防止过大的观测噪声导致滤波器发散。
		}
	}

	const float pos_var = sq(pos_noise);
	const Vector2f pos_obs_var(pos_var, pos_var);  // 方差  对角矩阵直接只存储对角线向量了
	updateHorizontalPositionAidSrcStatus(gnss_sample.time_us,        // 更新水平位置辅助变量
					     position,                                   // observation
					     pos_obs_var,                                // observation variance
					     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
					     aid_src);
}

// 控制对EKF-GSF中yaw的融合，根据水平速度
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
{
	// update yaw estimator velocity (basic sanity check on GNSS velocity data)
	const float vel_var = aid_src_vel.observation_variance[0];
	const Vector2f vel_xy(aid_src_vel.observation);

	if ((vel_var > 0.f)
	    && (vel_var < _params.req_sacc)
	    && vel_xy.isAllFinite()) {

		_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);

		// Try to align yaw using estimate if available
		if (((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
		     || (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
		    && !_control_status.flags.yaw_align
		    && _control_status.flags.tilt_align) {
			if (resetYawToEKFGSF()) {  // 重置EKF-GSF的估计到24-EKF
				ECL_INFO("GPS yaw aligned using IMU");
			}
		}
	}
}

// 用EKF-GSF重新估计yaw，并返回是否成功
bool Ekf::tryYawEmergencyReset()
{
	bool success = false;

	/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
	 * fails while the difference between the yaw emergency estimator and the yaw estimate is large.
	 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
	 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
	 */
	if (resetYawToEKFGSF()) {
		ECL_WARN("GPS emergency yaw reset");

		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
			// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
			// and cause another navigation failure
			_control_status.flags.mag_fault = true;
			_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
		}

#if defined(CONFIG_EKF2_GNSS_YAW)

		if (_control_status.flags.gps_yaw) {
			_control_status.flags.gps_yaw_fault = true;
			_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
		}

#endif // CONFIG_EKF2_GNSS_YAW

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

		if (_control_status.flags.ev_yaw) {
			_control_status.flags.ev_yaw_fault = true;
		}

#endif // CONFIG_EKF2_EXTERNAL_VISION

		success = true;
	}

	return success;
}

// 重置状态速度和方差至辅助变量中的值
void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
{
	_information_events.flags.reset_vel_to_gps = true;
	resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
	aid_src.time_last_fuse = _time_delayed_us;
}

// 重置状态水平位置和方差至辅助变量中的值
void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
{
	_information_events.flags.reset_pos_to_gps = true;
	resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
	_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
	aid_src.time_last_fuse = _time_delayed_us;
}

// 返回是不是要重置GPS融合，根据最近融合的时间戳是不是太老了，中间中断了一段时间
bool Ekf::shouldResetGpsFusion() const
{
	/* We are relying on aiding to constrain drift so after a specified time
	 * with no aiding we need to do something
	 */
	bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
					       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);  // 标志位：水平位置和速度一段时间没有进行融合了

#if defined(CONFIG_EKF2_OPTICAL_FLOW)

	if (has_horizontal_aiding_timed_out) {
		// horizontal aiding hasn't timed out if optical flow still active
		if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
			has_horizontal_aiding_timed_out = false;     // 如果有光流更新了则修改标志，
		}
	}

#endif // CONFIG_EKF2_OPTICAL_FLOW

	const bool is_reset_required = has_horizontal_aiding_timed_out
				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);  // 即使再放松一段时间阈值最新的也挺老，则将该标志位置true

	const bool is_inflight_nav_failure = _control_status.flags.in_air
					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
					     && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
					     && (_time_last_hor_vel_fuse > _time_last_on_ground_us)
					     && (_time_last_hor_pos_fuse > _time_last_on_ground_us); // 已起飞，上一个融合时间够老，并且是在空中融合的

	return (is_reset_required || is_inflight_nav_failure);
}

#if defined(CONFIG_EKF2_GNSS_YAW)  // 有双天线能够测量偏航角
// 控制对偏航角的融合
void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
{
	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
	    || _control_status.flags.gps_yaw_fault) {

		stopGpsYawFusion(); // 如果没有GNSS或者GNSS测yaw失效则停止融合
		return;
	}

	updateGpsYaw(gps_sample);  // 更新GNSS融合yaw的辅助变量

	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);

	if (is_new_data_available) {// 偏航角可用

		const bool continuing_conditions_passing = _control_status.flags.tilt_align;  // 完成了倾斜对准

		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
				2 * GNSS_YAW_MAX_INTERVAL);   // GNSS偏航数据是否长时间未更新

		const bool starting_conditions_passing = continuing_conditions_passing
				&& _gps_checks_passed
				&& !is_gps_yaw_data_intermittent
				&& !_gps_intermittent  // 在 controlGpsFusion 中赋值，判断GNSS数据是否长时间没有更新
				; // GNSS数据检查通过且其他方面都有效，开始融合条件

		if (_control_status.flags.gps_yaw) {

			if (continuing_conditions_passing) {

				fuseGpsYaw(gps_sample.yaw_offset); // 融合偏航角

				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);

				if (is_fusion_failing) {
					if (_nb_gps_yaw_reset_available > 0) {// 长时间没有融合了则重置
						// Data seems good, attempt a reset
						resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset);

						if (_control_status.flags.in_air) {
							_nb_gps_yaw_reset_available--;
						}

					} else if (starting_conditions_passing) {
						// Data seems good, but previous reset did not fix the issue
						// something else must be wrong, declare the sensor faulty and stop the fusion
						_control_status.flags.gps_yaw_fault = true;
						stopGpsYawFusion();

					} else {
						// A reset did not fix the issue but all the starting checks are not passing
						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
						stopGpsYawFusion();
					}

					// TODO: should we give a new reset credit when the fusion does not fail for some time?
				}

			} else {
				// Stop GPS yaw fusion but do not declare it faulty
				stopGpsYawFusion();
			}

		} else {
			if (starting_conditions_passing) {
				// Try to activate GPS yaw fusion
				// 第一次融合要重置并改变相关的标志位
				if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
					ECL_INFO("starting GPS yaw fusion");

					_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
					_control_status.flags.gps_yaw = true;
					_control_status.flags.yaw_align = true;

					_nb_gps_yaw_reset_available = 1;
				}
			}
		}

	} else if (_control_status.flags.gps_yaw
		   && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {

		// No yaw data in the message anymore. Stop until it comes back.
		stopGpsYawFusion();
	}
}

// 停止GNSS yaw的融合，改变标志位并重置相关辅助变量
void Ekf::stopGpsYawFusion()
{
	if (_control_status.flags.gps_yaw) {

		_control_status.flags.gps_yaw = false;
		resetEstimatorAidStatus(_aid_src_gnss_yaw);

		// Before takeoff, we do not want to continue to rely on the current heading
		// if we had to stop the fusion
		if (!_control_status.flags.in_air) {
			ECL_INFO("stopping GPS yaw fusion, clearing yaw alignment");
			_control_status.flags.yaw_align = false;

		} else {
			ECL_INFO("stopping GPS yaw fusion");
		}
	}
}
#endif // CONFIG_EKF2_GNSS_YAW

// 停止GPS融合相关变量，停止融合
void Ekf::stopGpsFusion()
{
	if (_control_status.flags.gps) {
		ECL_INFO("stopping GPS position and velocity fusion");
		resetEstimatorAidStatus(_aid_src_gnss_pos);
		resetEstimatorAidStatus(_aid_src_gnss_vel);
		_last_gps_fail_us = 0;
		_last_gps_pass_us = 0;

		_control_status.flags.gps = false;
	}

	stopGpsHgtFusion();   // 停止高度滤波器1-EKF的融合
#if defined(CONFIG_EKF2_GNSS_YAW)
	stopGpsYawFusion();  // 如果使用GPS偏航融合，停止融合
#endif // CONFIG_EKF2_GNSS_YAW

	_yawEstimator.reset();   // 重置EKF-GSF
}

// 判断是否可以用估计yaw，只有EKF-GSF可用并且估计方差小于预设值时
bool Ekf::isYawEmergencyEstimateAvailable() const
{
	// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
	// data and the yaw estimate has converged
	if (!_yawEstimator.isActive()) {
		return false;
	}

	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
}

// 判断Yaw是否失败：不能用EKF-GSF并且EKF的估计误差相比于EKF-GSF更大
bool Ekf::isYawFailure() const
{
	if (!isYawEmergencyEstimateAvailable()) {  // 不能估计yaw
		return false;
	}

	const float euler_yaw = getEulerYaw(_R_to_earth);
	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());  // EKF的yaw和EKF-GSF的yaw相差太多则认为yaw估计失败

	return fabsf(yaw_error) > math::radians(25.f);
}

// 用EKF-GSF的yaw估计值重置yaw的估计
bool Ekf::resetYawToEKFGSF()
{
	if (!isYawEmergencyEstimateAvailable()) {  // 要能使用EKF-GSF
		return false;
	}

	// don't allow reset if there's just been a yaw reset
	const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);  // 改变了yaw是否对齐的标志位
	const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);               // 姿态四元数发生了重置

	if (yaw_alignment_changed || quat_reset) {
		return false;           // 刚重置了不再重置
	}

	ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
		 (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());

	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());  // 用EKF-GSF的yaw估计值重置yaw的估计和方差

	_control_status.flags.yaw_align = true;    // 改为yaw对齐了
	_information_events.flags.yaw_aligned_to_imu_gps = true;

	return true;
}

// 从EKF-GSF中获得偏航角及方差，东向北向速度等
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
			float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}
